%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 参考文献 https://blog.csdn.net/aaa376070331/article/details/107029515/
% 加加速度 jerk = da/dt 其中a = v / t
% 匀加速段存在段条件是 末速度V - 初始速度V0 > 目标加速度A_max / 加速度过程中的位移H_max
% 关于加加速度的一些疑问，ChatGPT的回答是这样的

% 问 ： 加加速度随加速度的变化而变化，不需要人为的给定？
% 答 ： 加加速度是加速度对时间的导数，因此它会随加速度的变化而变化。
%       你不需要人为地给定加加速度，只需定义加速度的变化规律（如加速度是一个函数）即可。
%       根据这个定义，通过计算加速度对时间的导数，就能得到相应的加加速度。在实际应用中，
%       加速度曲线的变化自然会导致加加速度的变化。

% 运动学基本公式推导
% 1. v = v0 + at
% 2. s = v0*t + (1/2)*a*t^2
% 3. v^2 = v0^2 + 2as

% 推导过程：

% 1. 第一个公式 (v = v0 + at)
%    - 定义加速度 a 为速度 v 随时间 t 的变化率：
%      a = (v - v0) / t
%    - 重排得：
%      v = v0 + at
%    - 这个公式表明在加速度 a 作用下，经过时间 t，物体的速度 v 为初始速度 v0 加上加速度乘以时间。

% 2. 第二个公式 (s = v0*t + (1/2)*a*t^2)
%    - 物体的位移 s 可以通过速度的积分表示：
%      s = ∫v dt
%    - 在加速度为常数的情况下，速度 v 可以表示为：
%      v = v0 + at
%    - 将 v 代入位移公式中：
%      s = ∫(v0 + at) dt
%    - 计算积分：
%      s = v0*t + (1/2)*a*t^2
%    - 这个公式表示在时间 t 内的位移 s。

% 3. 第三个公式 (v^2 = v0^2 + 2as)
%    - 从第一个公式中得到时间 t 的表达式：
%      t = (v - v0) / a
%    - 将 t 代入第二个公式：
%      s = v0 * ((v - v0) / a) + (1/2)*a*((v - v0) / a)^2
%    - 化简：
%      s = (v0*(v - v0)/a) + (1/2)*a*((v - v0)^2 / a^2)
%      s = (v0*(v - v0)/a) + (1/2)*(v - v0)^2 / a
%    - 乘以 2a 得到：
%      2as = 2v0(v - v0) + (v - v0)^2
%    - 整理为：
%      v^2 = v0^2 + 2as

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 这个对S型加速度的模拟没有速度前瞻算法，所以 位移S,和时间T都用公式推导，使得满足算的要求。
% 首先给定出 目标速度，加速度, 
Amax = 100;
Vmax = 3000;
% 人为给定一个时间，保证在最高速度下运行一段时间
Tuv = 100;
% 设初始速度为 V0
V0 = 0;
% 计算出加速度段满足 从V0 加速度到Vmax 的时间要求 TAmax
TAmax = ( Vmax - V0 ) / Amax;

% 计算出加速到Vmax需要的行程
SAmax = V0 + V0 * TAmax + 1/2 * Amax * TAmax^2;
% 计算 加加速度, 减速段的加加速度与这个一致
Jmax = V / TAmax^2;

Jmax = Jmax * 2;

name = 'Jmax';   
str = sprintf('%s = %d',name, Jmax);
disp(str);

% 判断加速过程中是否存在 匀加速段 并计算各段的时间
% 这个判断的条件是 根据基本运动方程 V^2 = V_s^2 + 2 * a_avg * d得到的
if Vmax - V0 > Amax^2 / SAmax
    TA1 = Amax / Jmax;                      % 加加速度段
    TA2 = ( (Vmax - V0) / Amax ) - TA1;     % 匀加速度度段
    TA3 = TA1;                              % 减加速度段
else
    TA1 = sqrt( (2 * Vmax - 2 * V0) / Jmax );
    TA2 = 0;
    TA3 = TA1;
end

% 判断减速过程中是否存在 匀减速段 并计算各段的时间
% 减速曲线与加速曲线对称
if Vmax - V0 > Amax^2 / SAmax
    TD1 = Amax / Jmax;             % 加加速度段
    TD2 = (Vmax - V0) / Amax - TD1; % 匀加速度度段
    TD3 = TD1;                      % 减加速度段
else
    TD1 = sqrt( (2 * Vmax - 2 * V0) / Jmax );
    TD2 = 0;
    TD3 = TD1;
end

% 匀速时间段这里直接给定为Suv,不做约束

% 计算第一段
time_1 = 0 : 0.1 : TA1;
j_1 = ones( size( time_1 ) ) * Jmax;
a_1 = Jmax * time_1;
v_1 = V0 + 1/2 * Jmax * time_1.^2;
s_1 = V0 * time_1 + 1/6 * Jmax * time_1.^3;

% 计算第二段
time_2 = 0 : 0.1 : TA2;
j_2 = ones( size( time_2 ) ) * 0;
a_2 = ones( size( time_2 ) ) * Amax;
v_2 = v_1(end) + Amax * time_2;
s_2 = s_1(end) + v_1(end) * time_2 + 1/2 * Jmax * TA1 * time_2.^2;

% 计算第三段
time_3 = 0 : 0.1 : TA3;
j_3 = ones( size( time_1 ) ) * Jmax * -1;
a_3 = Amax - Jmax * time_3;
v_3 = v_2(end) + Amax * time_3 - 1/2 * Jmax * time_3.^2;
s_3 = s_2(end) + v_2(end) * time_3 + 1/2 * Jmax * TA1 * time_3.^2 - 1/6 * Jmax * time_1.^3;

% 计算第四段 - 匀速运动段
time_4 = 0 : 0.1 : Tuv;
j_4 = ones( size( time_4 ) ) * 0;
a_4 = ones( size( time_4 ) ) * 0;
v_4 = ones( size( time_4 ) ) * Vmax;
s_4 = s_3(end) + v_3(end) * time_4;

% 计算第五段
time_5 = 0 : 0.1 : TD1;
j_5 = ones( size( time_5 ) ) * Jmax * -1;
a_5 = -1 * Jmax * time_5;
v_5 = v_4(end) - 1/2 * Jmax * time_5.^2;
s_5 = s_4(end) + v_4(end) * time_5 - 1/6 * Jmax * time_5.^3;

% 计算第六段
time_6 = 0 : 0.1 : TD2;
j_6 = ones( size( time_6 ) ) * 0;
a_6 = ones( size( time_6 ) ) * -1 * Amax;
v_6 = v_5(end) - Amax * time_6;
s_6 = s_5(end) + v_5(end) * time_6 - 1/2 * Jmax * TD1 * time_6.^2;

% 计算第七段
time_7 = 0 : 0.1 : TD3;
j_7 = ones( size( time_7 ) ) * Jmax;
a_7 = -1 * Amax + Jmax * time_7;
v_7 = v_6(end) - Amax * time_7 + 1/2 * Jmax * time_7.^2;
s_7 = s_6(end) + v_6(end) * time_7 - 1/2 * Jmax * TD1 * time_7.^2 + 1/6 * Jmax * time_7.^3;


total_time = [ time_1, ...
               time_2 + TA1, ...
               time_3 + TA1 + TA2, ...
               time_4 + TA1 + TA2 + TA3, ...
               time_5 + TA1 + TA2 + TA3 + Tuv, ...
               time_6 + TA1 + TA2 + TA3 + Tuv + TD1, ...
               time_7 + TA1 + TA2 + TA3 + Tuv + TD1 + TD2];
total_displacement = [ s_1, s_2, s_3, s_4, s_5, s_6, s_7 ];
total_acceleration = [ a_1, a_2, a_3, a_4, a_5, a_6, a_7 ];
total_velocity = [ v_1, v_2, v_3, v_4, v_5, v_6, v_7 ];
total_jerk = [ j_1, j_2, j_3, j_4, j_5, j_6, j_7 ];
% 绘图
figure;
subplot( 2, 2, 1 );
plot(total_time, total_displacement, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位移 (mm)');      % y轴标签
title('总的位移');        % 图形标题
grid on;                  % 显示网格

subplot( 2, 2, 2 );
plot(total_time, total_acceleration, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位置 (mm)');      % y轴标签
title('总的加速度');        % 图形标题
grid on;                  % 显示网格

subplot( 2, 2, 3 );
plot(total_time, total_velocity, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位置 (mm)');      % y轴标签
title('总的速度');        % 图形标题
grid on;                  % 显示网格

subplot( 2, 2, 4 );
plot(total_time, total_jerk, 'r');
xlabel('时间 (s)');       % x轴标签
ylabel('位置 (mm)');      % y轴标签
title('总的加加速度');     % 图形标题
grid on;                  % 显示网格